/*
 * Trajectory.cpp
 *
 *  Created on: Oct 23, 2013
 *      Author: incubed
 */

// copy from base_local_planner/trajectory
#include <ssrr_ompl_planner/Trajectory.h>

namespace ssrr {

  Trajectory::Trajectory()
    : xv_(0.0), yv_(0.0), thetav_(0.0), goal_cost_(-1.0), path_cost_(-1.0), obstacle_cost_(-1.0),
      final_xv_(0.0), final_yv_(0.0), final_thetav_(0.0), test_(0.0)
  {
  }

  Trajectory::Trajectory(double xv, double yv, double thetav, unsigned int num_pts)
    : xv_(xv), yv_(yv), thetav_(thetav), goal_cost_(-1.0), path_cost_(-1.0), obstacle_cost_(-1.0),
      x_pts_(num_pts), y_pts_(num_pts), th_pts_(num_pts)
  {
  }

  Trajectory::Trajectory(const Trajectory& src) {
    resetData(src);
  }

  void Trajectory::resetData(const Trajectory& src) {
	  xv_ = src.xv_;
	  yv_ = src.yv_;
	  thetav_ = src.thetav_;
	  goal_cost_ = src.goal_cost_;
	  path_cost_ = src.path_cost_;
	  obstacle_cost_ = src.obstacle_cost_;

	  this->resetPoints();
	  double x, y, th;
	  for (unsigned int i = 0; i < src.getPointsSize(); i++) {
		src.getPoint(i, x, y, th);
		this->addPoint(x, y, th);
	  }

  }


  void Trajectory::getPoint(unsigned int index, double& x, double& y, double& th) const {
    x = x_pts_[index];
    y = y_pts_[index];
    th = th_pts_[index];
  }

  void Trajectory::setPoint(unsigned int index, double x, double y, double th){
    x_pts_[index] = x;
    y_pts_[index] = y;
    th_pts_[index] = th;
  }

  void Trajectory::addPoint(double x, double y, double th){
    x_pts_.push_back(x);
    y_pts_.push_back(y);
    th_pts_.push_back(th);
  }

  void Trajectory::resetPoints(){
    x_pts_.clear();
    y_pts_.clear();
    th_pts_.clear();
  }

  void Trajectory::getEndpoint(double& x, double& y, double& th){
    x = x_pts_.back();
    y = y_pts_.back();
    th = th_pts_.back();
  }

  unsigned int Trajectory::getPointsSize() const {
    return x_pts_.size();
  }

  void Trajectory::setGoalCost(double goal_cost) {
  	goal_cost_ = goal_cost;
  }

  void Trajectory::setPathCost(double path_cost) {
  	path_cost_ = path_cost;
  }

  void Trajectory::setObstacleCost(double obstacle_cost) {
  	obstacle_cost_ = obstacle_cost;
  }

  void Trajectory::updateObstacleCost(double obstacle_cost) {
  	obstacle_cost_ = std::max(obstacle_cost_, obstacle_cost);
  }

  double Trajectory::getCost() const {
  	if (goal_cost_ < 0. || path_cost_ < 0. || obstacle_cost_ < 0.) {
  		return -1.;
  	}
  	double goal_factor = 0.8;
  	double path_factor = 0.6;
  	double obstacle_factor = 3.;
  	return goal_cost_ * goal_factor + path_cost_ * path_factor + obstacle_cost_ * obstacle_factor;
  }

  double Trajectory::getGoalCost() const {
  	return goal_cost_;
  }

  double Trajectory::getPathCost() const {
  	return path_cost_;
  }

  double Trajectory::getObstacleCost() const {
  	return obstacle_cost_;
  }

  void Trajectory::print() const {
	  cout << "--- Trajectory::print() ---" << endl;
	  cout << "vx: " << xv_ <<  ", vy: " << yv_ << ", vth: " << thetav_ << ", cost: " << getCost() << "test: " << test_ << endl;
	  cout << "fvx: " << final_xv_ <<  ", fvy: " << final_yv_ << ", fvth: " << final_thetav_ << endl;

	  cout << "pointsSize: " << getPointsSize() << endl;
	  cout << "points: ";
	  for (unsigned int i = 0;  i < getPointsSize(); i++) {
		  cout << "[ " << x_pts_[i] << ", " << y_pts_[i] << ", " << th_pts_[i] << "]; ";
	  }
	  cout << endl;

	  cout << "---------------------------" << endl;
  }

};

